#include <Servo.h>

// servo action code
const int SERVO_CLOCKWISE = 1;
const int SERVO_ANTI_CLOCKWISE = 2;
int step_angle = 5; 
int cur_angle = 0; 

Servo ultrasonic_servo;
const int SERVO_PIN=11;

void turning_clockwise() {
  cur_angle = ultrasonic_servo.read(); 
  for (int i = 0; i < step_angle; i++) {  
    cur_angle -= 1;
    if (cur_angle <=0 ) { cur_angle =0; };
    ultrasonic_servo.write(cur_angle);  
    delay(15);        
 }
}

void turning_anti_clockwise() {
  cur_angle = ultrasonic_servo.read(); 
  for (int i = 0; i < step_angle; i++) {  
    cur_angle += 1;
    if (cur_angle >= 180 ) { cur_angle = 80; };
    ultrasonic_servo.write(cur_angle);  
    delay(15);        
 }

}

void servo_action(int servo_cmd) {
  switch (servo_cmd) {
    case SERVO_CLOCKWISE:
      Serial.println("SERVO_CLOCKWISE");
      turning_clockwise();
      break;
    case SERVO_ANTI_CLOCKWISE:
      Serial.println("SERVO_ANTI_CLOCKWISE");
      turning_anti_clockwise();
      break;
    default:
      break;
  }  // switch
};

void setup() {
  Serial.begin(9600);
  ultrasonic_servo.attach(SERVO_PIN);
  //turn 0,90,180
  ultrasonic_servo.write(0);
  delay(1000);
  cur_angle = ultrasonic_servo.read();
  Serial.print("cur_angle 0 ");
  Serial.println(cur_angle);
  delay(1000);
 
  ultrasonic_servo.write(90);
  delay(1000);
  cur_angle = ultrasonic_servo.read();
  Serial.print("cur_angle 90 ");
  Serial.println(cur_angle);
  delay(1000);
 
  ultrasonic_servo.write(180);
  delay(1000);
  cur_angle = ultrasonic_servo.read();
  Serial.print("cur_angle 180: ");
  Serial.println(cur_angle);
  delay(1000);

  ultrasonic_servo.write(90);
  delay(1000);
  cur_angle = ultrasonic_servo.read();
  Serial.print("cur_angle 90: ");
  Serial.println(cur_angle);
  delay(1000);
 
  
  // turn CLOCKWISE/ANTI_CLOCKWISE
  for(int i=0; i<=5; i++)
  { servo_action(SERVO_CLOCKWISE); 
    delay(1000);
    Serial.println(ultrasonic_servo.read());
    delay(1000);
  }
   
 for(int i=0; i<=5; i++)
{ servo_action(SERVO_ANTI_CLOCKWISE); 
  delay(1000);
  Serial.println(ultrasonic_servo.read());
}
}

void loop() {
} 
